#include "includes.h"

extern int gramb_error_code;

extern track_status gramb_track;
extern led_status led[4];

extern double servo_pid_k[3];
extern control_struct servo;

extern double motor_pid_k[3];
extern control_struct motor[2];

extern double duty_out[2];

extern control_struct anti[2];

extern f_io_type f_servo_in[1];
extern f_io_type f_servo_out[1];
extern f_mf_type f_servo_e_memf[4];
extern f_mf_type f_servo_duty_memf[4];
extern f_rule_type f_servo_rule[4];
extern f_rule_elemtype f_servo_rule_elem[4][2];

extern uint16 amp[4];
extern const uint8_t amp_ad[AMP_ADC_CHN+1];

void check_input(int check){
    if(check==-1){
        printf("[Input Error]\n");
        while(1);
    }
}

void variable_init(void){
    int i;

    /* clear error code */
    gramb_error_code=0x00;

    memset(&gramb_track, 0, sizeof(gramb_track));

    servo.e_int=0.0;
    /* apply servo PID parameter */
    memcpy(servo.k, servo_pid_k, sizeof(servo.k));
    servo.duty_out=SERVO_DUTY_INIT;
    servo.t=0;
    servo.u=0.0;

    for(i=0; i<2; ++i){
        motor[i].e_int=0.0;
        /* apply motor PID parameter */
        memcpy(motor[i].k, motor_pid_k, sizeof(motor[i].k));
        motor[i].duty_out=MOTOR_DUTY_INIT;
        motor[i].t=0;
        motor[i].u=0.0;

        /* clear motor anti duty */
        anti[i].duty_out=0;
    }

    duty_out[0]=duty_out[1]=0;

    led[0].port=GPIO_PORTB;
    led[0].ch=0;
    led[0].val=1;

    led[1].port=GPIO_PORTC;
    led[1].ch=16;
    led[1].val=0;

    led[2].port=GPIO_PORTC;
    led[2].ch=11;
    led[2].val=0;

    led[3].port=GPIO_PORTC;
    led[3].ch=8;
    led[3].val=0;

    servo_fuzzy_init();
}

void pin_muxing(void){
    /* UART1 */
    PORTC_PCR3&=~PORT_PCR_MUX_MASK;
    PORTC_PCR3|=PORT_PCR_MUX(3);
    PORTC_PCR4&=~PORT_PCR_MUX_MASK;
    PORTC_PCR4|=PORT_PCR_MUX(3);

    /* motor TPM PWM */                                 // TPM0
    PORTE_PCR24&=~PORT_PCR_MUX_MASK;                    // TPM0_CH0
    PORTE_PCR24|=PORT_PCR_MUX(3);
    PORTE_PCR25&=~PORT_PCR_MUX_MASK;                    // TPM0_CH1
    PORTE_PCR25|=PORT_PCR_MUX(3);
    PORTA_PCR5&=~PORT_PCR_MUX_MASK;                     // TPM0_CH2
    PORTA_PCR5|=PORT_PCR_MUX(3);
    PORTE_PCR30&=~PORT_PCR_MUX_MASK;                    // TPM0_CH3
    PORTE_PCR30|=PORT_PCR_MUX(3);

    /* servo TPM PWM */                                 // TPM1_CH0
    PORTA_PCR12&=~PORT_PCR_MUX_MASK;
    PORTA_PCR12|=PORT_PCR_MUX(3);

    /* TPM impulse counter external clock */            // TPM_CLKIN0
    PORTB_PCR16&=~PORT_PCR_MUX_MASK;
    PORTB_PCR16|=PORT_PCR_MUX(4);

    /* LPTMR0 impulse counter external clock */         // LPTMR0_ALT2
    PORTC_PCR5&=~PORT_PCR_MUX_MASK;
    PORTC_PCR5|=PORT_PCR_MUX(3);
}

void gramb_init(FSM_EVENT *event){
    int check;

    DisableInterrupts;

    printf("------------------------------\n");
    print_version();
    printf("\n");
    
    printf("Initializing Gram[b]...\n\n");

    printf("Initializing variables...");
    variable_init();
    printf("[OK]\n");

    printf("Setting IRQ priority...\n");
    set_irq_priority(12, 1); // UART0 has second priority
    set_irq_priority(22, 0); // PIT has priority of first

    printf("Setting PIN muxing...");
    pin_muxing();
    printf("[OK]\n");

    printf("\n");
    printf("Initializing modules...\n");

    printf("Enable UART0 Interrupt...");
    check=0;
    check|=uart_irq(UART0, UART_IRQ_ENABLE);
    check_input(check);
    printf("[OK]\n");

    printf("Initializing GPIO...");
    check=0;
    /* LED port */
    check|=gpio_init(led[0].port, led[0].ch, GPIO_GPO, led[0].val); // On-Chip LED
    check|=gpio_init(led[1].port, led[1].ch, GPIO_GPO, led[1].val); // Red LED
    check|=gpio_init(led[2].port, led[2].ch, GPIO_GPO, led[2].val); // Green LED
    check|=gpio_init(led[3].port, led[3].ch, GPIO_GPO, led[3].val); // Blue LED
    /* input port */
    check|=gpio_init(GPIO_PORTC, 1, GPIO_GPI, 0);
    check|=gpio_init(GPIO_PORTD, 5, GPIO_GPI, 0);
    check|=gpio_init(GPIO_PORTD, 4, GPIO_GPI, 0);
    check|=gpio_init(GPIO_PORTD, 3, GPIO_GPI, 0);
    check|=gpio_init(GPIO_PORTD, 2, GPIO_GPI, 0);
    check|=gpio_init(GPIO_PORTD, 1, GPIO_GPI, 0);

    printf("Initializing LPTMR...");
    check=0;
    /* impulse counter 0 */
    check|=lptmr_init(LPTMR_ALT2, LPTMR_USE_OSCERCLK);
    check_input(check);
    printf("[OK]\n");

    printf("Initializing PIT...");
    check=0;
    /* control routine */
    check|=pit_init_ms(PIT0, PIT0_PERIOD);
    check_input(check);
    /* System rountine */
    check|=pit_init_ms(PIT1, PIT1_PERIOD);
    check_input(check);
    printf("[OK]\n");

    printf("Initializing ADC...");
    check=0;
    check|=adc_init(ADC_16bit, HIGH_SPEED, ADCK_24, AVGS_32, ADC_DMA_EN);
    check_input(check);
    if(check==-2){
        printf("[Calibration Failed]\n");
        while(1);
    }
    printf("[OK]\n");

    printf("Initializing TPM...\n");

    printf("  ");
    printf("Initializing motor PWM...");
    check=0;
    check|=tpm_pwm_init(TPM0, TPM0_FREQ_INIT);
    check_input(check);
    if(check==-2){
        printf("[Frequency Out of Range]\n");
        while(1);
    }
    check|=tpm_pwm_duty(TPM0, TPM_CH0, TPM0_CH0_DUTY_INIT);
    check|=tpm_pwm_duty(TPM0, TPM_CH1, TPM0_CH1_DUTY_INIT);
    check|=tpm_pwm_duty(TPM0, TPM_CH2, TPM0_CH2_DUTY_INIT);
    check|=tpm_pwm_duty(TPM0, TPM_CH3, TPM0_CH3_DUTY_INIT);
    check_input(check);
    if(check==-2){
        printf("[TPM%d is not in Edeg-aligned PWM Mode]\n", TPM0);
        while(1);
    }
    printf("[OK]\n");

    printf("  ");
    printf("Initializing servo PWM...");
    check=0;
    check|=tpm_pwm_init(TPM1, TPM1_FREQ_INIT);
    check_input(check);
    if(check==-2){
        printf("[Frequency Out of Range]\n");
        while(1);
    }
    check|=tpm_pwm_duty(TPM1, TPM_CH0, TPM1_CH0_DUTY_INIT);
    check_input(check);
    if(check==-2){
        printf("[TPM%d is not in Edge-aligned PWM Mode]\n", TPM1);
        while(1);
    }
    printf("[OK]\n");

    printf("  ");
    printf("Initializing TPM2 counter...");
    check=0;
    /* impulse counter 1 */
    check|=tpm_cnt_init(TPM2);
    check_input(check);
    printf("[OK]\n");

    printf("Initializing ADC DMA...");
    check=0;
    check|=dma_periph_init(DMA_CH0, DMAMUX_ADC0, &ADC0_RA, (uint32_t *)amp, AMP_ADC_CHN*2, DMA_16bit, DMA_16bit, DMA_CYCL_MASK|DMA_DINC_MASK);
    check|=dma_memory_init(DMA_CH1, (uint32_t *)amp_ad, &ADC0_SC1A, AMP_ADC_CHN+1, DMA_8bit, DMA_8bit, DMA_CYCL_MASK|DMA_SINC_MASK);
    DMA_SAR0=(uint32_t)&ADC0_RA;
    DMA_DAR1=(uint32_t)&ADC0_SC1A;
    check|=dma_linkcc(DMA_CH0, 2, DMA_CH1, DMA_CH0);
    check|=dma_irq_enable(DMA_CH1);
    check|=dmamux_enable(DMAMUX_CH0);
    check|=dmamux_enable(DMAMUX_CH1);
    check_input(check);
    dma_adc_activate();
    printf("[OK]\n");

    printf("\nInitialization complete.\n");

    printf("------------------------------\n");

    event_buffer_push(event, GRAMB_EVENT_RESUME);

    EnableInterrupts;
}
